#include "./rs7020.h"
#include "../common/common_modbus.h"
#include "../common/common_motor.h"
#include "../../bsp/cli_cmd.h"

static int __local_assert(xp_motor_t *motor)
{
    if (motor->inited != 1 ||
        motor->private_data == NULL ||
        motor->ops == NULL)
    {
        println("motor not inited.");
        return -1;
    }
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    if (mb_dev->bus == NULL)
    {
        println("motor %s not mount in bus of modbus", mb_dev->name);
        return -2;
    }
    return 0;
}

#define LOCAL_ASSERT(motor)    \
    if (__local_assert(motor)) \
    return -1

static int rs7020_init(xp_motor_t *motor)
{
    // 配置电机加速度、减速带、使能等
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;

    unsigned short data = 0x0083;
    unsigned short addr = 0x0405; // 配置0x83为内部使能，配置为0x03需要外部接线使能
    int ret = bus->swrite(mb_dev, addr, data);
    // 配置电机为速度模式
    data = 0x0002;
    addr = 0x6000;
    ret = bus->swrite(mb_dev, addr, data);

    return ret;
}

static int rs7020_speed_set(xp_motor_t *motor, motor_rpm_t rpm)
{
    LOCAL_ASSERT(motor);

    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct rs7020_motor_attr *motor_attr = (struct rs7020_motor_attr *)(mb_dev->device_attr);

    unsigned short speed_addr = 0x6203;
    short speed = (motor_attr->dir == motor_forward) ? rpm : 0 - rpm;
    int ret = bus->swrite(mb_dev, speed_addr, speed);
    if (0 == ret)
        motor_attr->rpm = rpm;

    if (motor_attr->running) ret = motor->ops->rotation_start(motor);

    return ret;
}

static int rs7020_speed_get(xp_motor_t *motor, motor_rpm_t *rpm)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct rs7020_motor_attr *motor_attr = (struct rs7020_motor_attr *)(mb_dev->device_attr);

    short speed = 0;
    unsigned short speed_addr = 0x6203;
    int ret = bus->sread(mb_dev, speed_addr, (unsigned short *)&speed);
    if (0 == ret)
    {
        if (speed < 0)
        {
            motor_attr->dir = motor_reverser;
            motor_attr->rpm = 0 - speed;
        }
        else
        {
            motor_attr->dir = motor_forward;
            motor_attr->rpm = speed;
        }
    }
    *rpm = speed;

    return ret;
}

static int rs7020_direction_set(xp_motor_t *motor, motor_direction_t dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct rs7020_motor_attr *motor_attr = (struct rs7020_motor_attr *)(mb_dev->device_attr);

    short rpm = motor_attr->rpm;
    unsigned short speed_addr = 0x6203;
    if (dir == motor_reverser)
        rpm = 0 - rpm;
    int ret = bus->swrite(mb_dev, speed_addr, rpm);
    if (0 == ret)
        motor_attr->dir = dir;

    if (motor_attr->running) motor->ops->rotation_start(motor);

    return ret;
}

static int rs7020_direction_get(xp_motor_t *motor, motor_direction_t *dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct rs7020_motor_attr *motor_attr = (struct rs7020_motor_attr *)(mb_dev->device_attr);

    *dir = motor_attr->dir;

    return 0;
}

static int rs7020_rotate_by_angle(xp_motor_t *motor, int angle)
{

    return 0;
}

static int rs7020_start(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct rs7020_motor_attr *motor_attr = (struct rs7020_motor_attr *)(mb_dev->device_attr);

    short speed = 0x0000;
    unsigned short run_addr = 0x6002;
    unsigned short speed_addr = 0x6003;
    unsigned short start = 0x0010;
    if (motor_attr->dir == motor_forward)
        speed = motor_attr->rpm;
    else
        speed = 0 - motor_attr->rpm;
    int ret = bus->swrite(mb_dev, speed_addr, (unsigned short)speed);
    if (0 == ret)
        ret = bus->swrite(mb_dev, run_addr, start);
    if (0 == ret) motor_attr->running = 1;
    return ret;
}

static int rs7020_stop(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct rs7020_motor_attr *motor_attr = (struct rs7020_motor_attr *)(mb_dev->device_attr);

#if 0
    motor_attr->rpm = 0;
    motor_rpm_t speed = motor_attr->rpm;
    unsigned short run_addr = 0x6002;
    unsigned short speed_addr = 0x6003;
    int ret = bus->swrite(mb_dev, speed_addr, speed);
    aos_msleep(500);
    unsigned short stop = 0x0010;
    if (0 == ret)
        ret = bus->swrite(mb_dev, run_addr, stop);
#endif
    int ret = rs7020_speed_set(motor, 0);

    if (0 == ret) motor_attr->running = 0;

    return ret;
}

static int rs7020_emerg_enable(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct rs7020_motor_attr *motor_attr = (struct rs7020_motor_attr *)(mb_dev->device_attr);

    unsigned short emerg = 0x0040;
    unsigned short emerg_addr = 0x6002;
    int ret = bus->swrite(mb_dev, emerg_addr, emerg);

    if (0 == ret) motor_attr->running = 0;

    return ret;
}

static int rs7020_emerg_disable(xp_motor_t *motor)
{
    return 0; // 不需要恢复
}

static int rs7020_error_get(xp_motor_t *motor, motor_error_t *err_code)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;

    unsigned short error_addr = 0x0000;

    int ret = bus->sread(mb_dev, error_addr, err_code);

    return ret;
}

static int rs7020_error_clear(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    return 0;
}

struct xp_motor_stdops rs7020_stdops = {
    .init = rs7020_init,
    .speed_set = rs7020_speed_set,
    .speed_get = rs7020_speed_get,
    .direction_set = rs7020_direction_set,
    .direction_get = rs7020_direction_get,
    .rotate_by_angle = rs7020_rotate_by_angle,
    .rotation_start = rs7020_start,
    .rotation_stop = rs7020_stop,
    .emerg_enable = rs7020_emerg_enable,
    .emerg_disable = rs7020_emerg_disable,
    .error_get = rs7020_error_get,
    .error_clear = rs7020_error_clear,
};

int xp_rs7020_constructor(struct xp_motor *motor, struct xp_modbus_device *dev, unsigned char *name)
{
    if (motor == NULL || dev == NULL || name == NULL)
        return -1;
    motor->private_data = (void *)dev;
    strncpy((char *)motor->name, (char *)name, XP_MOTOR_NAME_LENGTH);
    motor->ops = &rs7020_stdops;

    if (NULL == motor->ops->init || 0 != motor->ops->init(motor))
    {
        motor->inited = 0;
        return -1;
    }

    motor->inited = 1;
    return 0;
}

/*********** DEBUG TEST ************/
#if (XP_CONFIG_AGV_WHEEL_RS7020_ENABLE == 1)
struct xp_motor rs7020_debug_motor_1 = {0};
struct xp_motor rs7020_debug_motor_2 = {0};

#include "../../washer/device_manager.h"
int xp_rs7020_debug(char *type, char *fun, char *param)
{
    if (strcmp(type, "rs7020-1") != 0 &&
        strcmp(type, "rs7020-2") != 0)
        return 0;

    int ret = 0;

    struct xp_modbus_device *rs7020_debug_dev_1 = get_modbus_device_by_name((unsigned char *)"rs7020_leftwheel");
    struct xp_modbus_device *rs7020_debug_dev_2 = get_modbus_device_by_name((unsigned char *)"rs7020_rightwheel");

    if (rs7020_debug_dev_1 == NULL || rs7020_debug_dev_2 == NULL)
        return 0;

    if (rs7020_debug_dev_1->inited != xp_true || rs7020_debug_dev_2->inited != xp_true)
        return 0;

    ret = xp_rs7020_constructor(&rs7020_debug_motor_1, rs7020_debug_dev_1, (unsigned char *)"rs7020_debug_1");
    ret |= xp_rs7020_constructor(&rs7020_debug_motor_2, rs7020_debug_dev_2, (unsigned char *)"rs7020_debug_2");
    if (ret != 0)
        return 0;
    motor_direction_t dir = motor_forward;
    int id = 0;

    char *p = strstr(type, "-");
    if (p == NULL)
        return 0;
    p++;
    id = atoi(p);

    struct xp_motor rs7020_debug_motor = {0};

    if (id == 1)
        rs7020_debug_motor = rs7020_debug_motor_1;
    else if (id == 2)
        rs7020_debug_motor = rs7020_debug_motor_2;
    else
        return 0;

    if (strcmp(fun, "set_speed") == 0)
    {
        println("set motor rpm = %d", atoi(param));
        ret = rs7020_debug_motor.ops->speed_set(&rs7020_debug_motor, atoi(param));
    }
    else if (strcmp(fun, "set_dir") == 0)
    {
        if (atoi(param) == 1)
            dir = motor_reverser;
        println("set motor dir = %d", dir);
        ret = rs7020_debug_motor.ops->direction_set(&rs7020_debug_motor, dir);
    }
    else if (strcmp(fun, "start") == 0)
    {
        println("set motor start");
        ret = rs7020_debug_motor.ops->rotation_start(&rs7020_debug_motor);
    }
    else if (strcmp(fun, "stop") == 0)
    {
        println("set motor stop");
        ret = rs7020_debug_motor.ops->rotation_stop(&rs7020_debug_motor);
    }
    else if (strcmp(fun, "emerg") == 0)
    {
        println("set motor emerg");
        ret = rs7020_debug_motor.ops->emerg_enable(&rs7020_debug_motor);
    }

    return 1;
}
#endif
